#include "hc_sr04.h"


#include <stdbool.h>
#include <stdlib.h>

#include <TM4C123GH6PM.h>
#include "driverlib/sysctl.h"
#include "driverlib/timer.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/pwm.h"
#include "driverlib/interrupt.h"
#include "inc/hw_ints.h"

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

#include "myPWM/myPWM.h"
#include "myCAP/myCap.h"
#include "mySerial/mySerial.h"
#include "MOTOR/motor.h"

#define MEASURETIME_MAX 17647 /* 最大测量声波时间长度(us) 17647us即最大测量距离为3米*/

uint32_t measureTime = MEASURETIME_MAX; // 输入捕获状态 bit15表示是否完成一次脉冲捕获，bit14表示是否完成脉冲第一次变化沿，bit13~bit0表示脉冲持续时间(T0CCP1_STA++语句触发周期)
QueueHandle_t distance_queue = NULL;

/**
 * @brief    Ranging_Task 任务函数
 *     获取小车与前方物体间距离
 */
void Ranging_Task(void *pvParameters)
{
  float distance_mm;
  uint32_t measureTime;
  TimerEnable(TIMER0_BASE, TIMER_A);
  while (1)
  {
    if(xQueueReceive(distance_queue, &measureTime, 100/portTICK_RATE_MS) == pdTRUE){
      distance_mm = measureTime*0.17; // 计算出毫米长度    mm = us/1000 * 340*1000/1000000
      HC_SR04_Stop();
      printf("[I] Distance: %.2fmm\n", distance_mm);
      if(distance_mm < 50){
        Motor_SetSpeed(-200, -200);
        vTaskDelay(300);
        Motor_SetSpeed(0, 0);
      }
    }
    else{
      HC_SR04_Start();
    }
  }
}

/**
 * @brief    HC_SR04 模块初始化，其实做的就是初始化PWM控制Trig引脚和初始化一个cap捕获Echo引脚的脉冲
 * @param    void
 * @retval   void
 */
void HC_SR04_Init(void)
{
  distance_queue = xQueueCreate(1, sizeof(uint16_t));
  M1PWM4_Init(SYSCTL_PWMDIV_64, 0xFFFF, 13); // PWM周期为65535拉满，脉冲宽度为10us
  PWMGenDisable(PWM1_BASE, PWM_GEN_2);

  /* 配置Timer0A用于计时 */
  SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
  TimerConfigure(TIMER0_BASE, TIMER_CFG_SPLIT_PAIR | TIMER_CFG_A_PERIODIC_UP); // 配置定时器，将定时器拆分，并配置拆分后的定时器A为周期性计数,B模块为边沿触发模式|加计数模式
  TimerLoadSet(TIMER0_BASE, TIMER_A, 80); // 设置定时器装载值
//  TimerMatchSet(TIMER0_BASE, TIMER_A, 80);
  IntPrioritySet(INT_TIMER0A, 2<<5); // 设置中断优先级
  TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT); // 定时器中断允许，中断事件为计数器A计数溢出，Capture模式中边沿触发
  IntEnable(INT_TIMER0A); // NVIC中允许定时器模块中断
  TimerDisable(TIMER0_BASE, TIMER_A); // 失能定时器

  /* 配置PF1的外部中断 */
  SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
	GPIODirModeSet(GPIOF_BASE, GPIO_PIN_1, GPIO_DIR_MODE_IN);
  GPIOPadConfigSet(GPIOF_BASE, GPIO_PIN_1, GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPD);
  GPIOIntTypeSet(GPIOF_BASE, GPIO_PIN_1, GPIO_BOTH_EDGES); // 上升沿触发
  IntPrioritySet(INT_GPIOF, 1<<5);
	GPIOIntEnable(GPIOF_BASE, GPIO_INT_PIN_1);
	IntEnable(INT_GPIOF);

  // 使能系统总中断
  IntMasterEnable();
  //HC_SR04_Start();
  /* 创建测距任务 */
  xTaskCreate(Ranging_Task, "Ranging_Task", 128, NULL, 3, NULL);
}

/**
 * @brief    启动 HC_SR04 模块(未验证)
 * @param    void
 * @retval   void
 */
void HC_SR04_Start(void)
{
  // 使能定时器
  TimerEnable(TIMER0_BASE, TIMER_A);
  // 使能PWM发生器
  PWMGenEnable(PWM1_BASE, PWM_GEN_2);
}

/**
 * @brief    关闭 HC_SR04 模块(未验证)
 * @param    void
 * @retval   void
 */
void HC_SR04_Stop(void)
{
  // 失能PWM发生器
  PWMGenDisable(PWM1_BASE, PWM_GEN_2);
  // 失能定时器
  TimerDisable(TIMER0_BASE, TIMER_A);
}


/**
 * @brief us级定时器，用于测距
 *
 */
void TIMER0A_Handler(void)
{
  TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
  if(measureTime < MEASURETIME_MAX)
    measureTime++;
}

// GPIOF1外部中断函数
void GPIOF_Handler(void){
  GPIOIntClear(GPIOF_BASE, GPIOIntStatus(GPIOF_BASE, true));

  if(GPIOPinRead(GPIOF_BASE, GPIO_PIN_1) == GPIO_PIN_1){
    measureTime = 0;
  }
  else{
    if(measureTime > MEASURETIME_MAX)
      return;
    xQueueSendFromISR(distance_queue, &measureTime, 0);
    measureTime = MEASURETIME_MAX;
  }
}
